/*!
  @file solver_method_arun.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _SOLVER_METHOD_ARUN_H_
#define _SOLVER_METHOD_ARUN_H_

#include <Eigen/Dense>

#include "mrover_vo/solver_method.h"

namespace vo {

/*! 
 *  @brief  The three point alogithm [Arun 19..]
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  Reference
 *  Arun, 
 */

template <typename ValType>
class SolverMethodArun : public SolverMethod<ValType> {
 public:
  typedef Eigen::Matrix<ValType, Eigen::Dynamic, Eigen::Dynamic> MatrixXT;
  typedef Eigen::Matrix<ValType, 3, 1> Vector3T;
  typedef Eigen::Transform<ValType, 3, Eigen::Affine> Affine3T;
  typedef SolverMethod<ValType> BaseClass;

  using BaseClass::K;
  using BaseClass::input_;
  using BaseClass::target_;
  using BaseClass::matches_;
  using BaseClass::poses_;

  SolverMethodArun() { }

  virtual ~SolverMethodArun() { }

  virtual size_t getRequiredSampleSize() { return 7; }

  virtual bool checkGoodness(const std::vector<int> &samples, const int new_idx) {
    return true;
  }

 private:
  virtual bool initCompute() {
    if (!BaseClass::initCompute()) return false;
    if (matches_->size() < getRequiredSampleSize()) return false;
    return true;
  }

  virtual void initializeData(const std::vector<int> &samples);
  virtual void computePose();

  MatrixXT X;
  MatrixXT Y;
  size_t num_sample_;
};

} // namespace vo

#endif  // _SOLVER_METHOD_ARUN_H_
